CON
   _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000
   txpin=12
   rxpin=13
   loopcnt = 10000

OBJ
  i2c : "I2C_Bus"
  pst: "Parallax Serial Terminal"
  imu: "imu"
  
PUB Main | roll, pitch, bear, ok, i
 ' i2c.Display
  pst.startrxtx(rxpin, txpin, %000, 9600)    
  i2c.Start
  i := 0
  ok := imu.Start
  pst.dec(ok)

  repeat
    i++
      'pst.str(String("test"))
    if ina[i2c#GYROINT] == 1
      UpdateIMU
      imu.GetData(@pitch, @roll, @bear)
     ' if i > loopcnt
        
        'pst.str(String("Pitch: "))
        'pst.bin(pitch, 16)
        pst.dec(pitch)
        pst.str(String(" "))
        pst.dec(roll)              
       'pst.str(String(" Roll: "))
        'pst.dec(roll)
        'pst.str(String(" Bearing: "))
        'pst.dec(bear)
        'pst.char(pst#NL)
        pst.str(String("|"))
        i := 0 
  

Pub UpdateIMU| accX, accY, accZ, gyroX, gyroY, gyroZ
  i2c.GetData(@gyroX, @gyroY, @gyroZ, @accX, @accY, @accZ)
  imu.update(gyroX, gyroY, gyroZ, accX, accY, accZ)